#include <iostream>
#include <fstream>
#include <boost/format.hpp>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;
int main()
{

    const Mat Kl = ( Mat_<double> ( 3,3 ) << 689.8476, -0.2515,   620.8508, 
						                                        0.0,      691.1252, 489.8400,
 						                                        0.0,      0.0,      1.0 );
    const Mat Dl = ( Mat_<double> ( 5,1 ) << -0.3592, 0.1235, 2.9688e-06, -0.0015, -0.0185);
    const Mat Kr = ( Mat_<double> ( 3,3 ) << 680.8634, -0.1837,  655.5514, 
						                                        0.0,      682.0648, 501.6723,
						                                        0.0,      0.0,      1.0 );
    const Mat Dr = ( Mat_<double> ( 5,1 ) << -0.3642, 0.1285, 7.0494e-04, 0.0012, -0.0199);
    
    const Mat Rr2l = ( Mat_<double> ( 3,3 ) << 0.9997,	 -0.0020,    0.0246,
						                                         0.0020,  1.0000,     -0.0024,
			                                    			        -0.0245,  0.0025, 0.9997 );
    //const Mat Tr2l = ( Mat_<double> ( 3,1 ) << -106.2563, 0.0997, 1.7462 );
    const Mat Tr2l = ( Mat_<double> ( 3,1 ) << -1.062491e-1, 2.033e-4, 1.1014e-3);
    const int nImage = 1000;
    const int ImgWidth = 1280;
    const int ImgHeight = 1024;
	
    Size imageSize(ImgWidth, ImgHeight);
    Size undistort_imageSize(1280,1024); 
    const double alpha = -1;
    Mat R1,R2,P1,P2,Q;
    Mat rectifyImageL,rectifyImageR;
    Rect validRoiL,validRoiR;
    Mat rmap[2][2];
    
    stereoRectify(Kl,Dl,Kr,Dr,imageSize,Rr2l,Tr2l,R1,R2,P1,P2,Q,CALIB_ZERO_DISPARITY,alpha,undistort_imageSize,&validRoiL,&validRoiR);
    initUndistortRectifyMap(Kl,Dl,R1,P1,imageSize,CV_8UC1,rmap[0][0],rmap[0][1]);
    initUndistortRectifyMap(Kr,Dr,R2,P2,imageSize,CV_8UC1,rmap[1][0],rmap[1][1]);

    //cv::Mat NewCameraMatrix = getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
    //initUndistortRectifyMap(K, D, cv::Mat(), NewCameraMatrix, imageSize, CV_8UC1, map1, map2);
    
    boost::format fmt1("./00/image_0/%06dl.bmp");
    boost::format fmt2("./00/image_0_undistort/%06dl_undistort.bmp");
    boost::format fmt3("./00/image_1/%06dr.bmp");
    boost::format fmt4("./00/image_1_undistort/%06dr_undistort.bmp"); 
    
   
    cout<<"raw_Kl:"<<Kl<<endl;
    cout<<"new_Kl"<<P1<<endl;

    cout<<"raw_Kr"<<Kr<<endl;
    cout<<"new_Kr"<<P2<<endl;

    cout<<"new_Rl"<<R1<<endl;
    cout<<"new_Rr"<<R2<<endl;
    cout<<"raw_Rr2l"<<Rr2l<<endl;
    for(int i=1; i<nImage; i++)
    {
        string InputPathL = (fmt1 % i).str();
	string InputPathR = (fmt3 % i).str();
	Mat RawImageL = imread(InputPathL,IMREAD_GRAYSCALE);
        //imshow("Left_RawImage", RawImageL);
	
	Mat RawImageR = imread(InputPathR,IMREAD_GRAYSCALE);
	//imshow("Right_RawImage",RawImageR);
	
        Mat UndistortImageL,UndistortImageR;

        remap(RawImageL, UndistortImageL, rmap[0][0], rmap[0][1], INTER_LINEAR);
        //imshow("Left_UndistortImage", UndistortImageL);
        string OutputPathL = (fmt2 % i).str();
	imwrite(OutputPathL, UndistortImageL);	

	remap(RawImageR, UndistortImageR, rmap[1][0], rmap[1][1], INTER_LINEAR);
	//imshow("RIght_UndistortImage", UndistortImageR);
	string OutputPathR = (fmt4 % i).str();
        imwrite(OutputPathR, UndistortImageR);
 	
	// cout<<"Right_RawImageCOL"<<RawImageR.cols<<endl;
	// cout<<"Right_RawImageROW"<<RawImageR.rows<<endl;
	
	// cout<<"Right_UndistortImageCOL"<<UndistortImageR.cols<<endl;
	// cout<<"Right_UndistortImageROW"<<UndistortImageR.rows<<endl;


	// cout<<"Left_RawImageCOL"<<RawImageL.cols<<endl;
	// cout<<"Left_RawImageROW"<<RawImageL.rows<<endl;

	// cout<<"Left_UndistortImageCOL"<<UndistortImageL.cols<<endl;
	// cout<<"Left_UndistortImageROW"<<UndistortImageL.rows<<endl;
        //waitKey(0);
    }

    return 0;
} 

